package jcolorlinez;

import java.util.ArrayList;
import java.util.List;
import java.util.PriorityQueue;

import jcolorlinez.Grid.Cell;
import jcolorlinez.Grid.Direction;
import jcolorlinez.Grid.PathFinder;

public class DefaultPathFinder implements PathFinder {
	@Override
	public List<Cell> findPath(Grid grid, Cell src, Cell dst){
		if (!grid.contains(src)) {
			throw new IllegalArgumentException(src + "doesn't exist in:" + grid);
		}
		if (!grid.contains(dst)) {
			throw new IllegalArgumentException(dst + "doesn't exist in:" + grid);
		}
		if (src.equals(dst)) {
			throw new IllegalArgumentException("src: " + src + "dst: " + dst + "shouldn't be the same");
		}
		PriorityQueue<Path> queue = new PriorityQueue<>();
		int[][] occupied = grid.toMatrix();
		Path path = new Path(src, dst);
		queue.add(path);

		while ((path = queue.poll()) != null) {
			if (Main.DEBUG) {
				System.out.println("poll:");
				System.out.println("\t" + path);
				System.out.println("From:");
				for (Path p : queue) {
					System.out.println("\t" + p);
				}
				System.out.println();
			}
			
			Cell current = path.getLastCell();
			if (current.isAdjacentTo(dst)) {
				path.add(dst);
				return path.getCells();
			}
			
			boolean pathAdded = false;
			for (Direction d : Direction.mainValues) {
				Cell next = current.step(d);
				if (!grid.contains(next)) {
					continue;
				}
				if (occupied[next.row][next.col] != 0) {
					continue;
				}
				//Let the currently best path occupy the cell.
				//Although not always the best, should be a second best one
				occupied[next.row][next.col] = 1;
				if (!pathAdded) { //First time: use path directly
					path.add(next);
					queue.add(path);
					pathAdded = true;
				} else {          //Second time: clone path and reset last cell with new "next"
					Path path2 = new Path(path);
					path2.setLastCell(next);
					queue.add(path2);
				}
			}
		}
		return null;
	}
	
	private static class Path implements Comparable<DefaultPathFinder.Path> {
		private List<Cell> cells = null;
		private Cell dst = null;
		private int turns = -1;
		@SuppressWarnings("unused")
		private Path() {}
		public Path(Cell src, Cell dst) { //Make sure it has at least 1 element;
			cells = new ArrayList<>();
			cells.add(src);
			this.dst = dst;
		}
		public Path(Path old) {
			cells = new ArrayList<>(old.cells);
			dst = old.dst;
			turns = old.turns;
		}
		public void add(Cell cell) {
			assert(this.getLastCell().isAdjacentTo(cell));
			if (getDirection(-1) != cell.getDirectionFrom(getLastCell())) {
				turns++;
			}
			cells.add(cell);
		}
		public List<Cell> getCells() {
			return cells;
		}
		public Cell getLastCell() {
			return /*(cells.size() == 0) ? null :*/ cells.get(cells.size()-1);
		}
		public void setLastCell(Cell cell) {
			assert(!cell.equals(getLastCell()));
			if (getDirection(-1) == getDirection(-2)) {
				turns++;
			} else if (cell.getDirectionFrom(get(-2)) == getDirection(-2)) {
				turns--;
			}
			cells.set(cells.size()-1, cell);
		}
		private int distance() {
			return cells.size() + getLastCell().distance(dst);
		}
		private Direction getDirection(int index) { // -1 returns the last
			return get(index).getDirectionFrom(get(index-1));
		}
		private Cell get(int index) { // -1 returns the last
			if (index < 0) {
				index += cells.size();
			}
			if (index < 0 || index >= cells.size()) {
				return null;
			}
			return cells.get(index);
		}

		@Override
		public int compareTo(Path o) {
			int d1 = this.distance();
			int d2 = o.distance();
			if (d1 != d2) {
				return d1 - d2;
			}
			if (this.cells.size() != o.cells.size()) {
				return this.cells.size() - o.cells.size();
			}
			return this.turns - o.turns;
		}
		@Override
		public String toString() {
			StringBuilder str = new StringBuilder();
			str.append(this.getClass().getSimpleName()).append("->" + dst).append(":");
			for (Cell p : cells) {
				str.append("[" + p.row + "," + p.col + "]");
			}
			str.append(" turns:" + turns);
			return str.toString();
		}
	}
}